#include <TimerOne.h>

// shared code
// ================================================
// PWM
// ================================================
#define MAX_PWM_PIN 6
#define MAX_PWM_DUTY 100  // also pwm resolution

static struct {
  byte n;
  byte dutyCount;  // tick tok count
  byte dutyCycle[MAX_PWM_PIN];
  byte pin[MAX_PWM_PIN];
} pwmInfo;


void PWM_Init(int freq){
  // period = 1000*1000 us / freq / MAX_PWM_DUTY = 10000 / freq
  unsigned long period = 1000L*1000L / freq / MAX_PWM_DUTY;  // us
  Timer1.initialize(period);
  Timer1.attachInterrupt(PWM_Accumulate);

  Serial.print("freq: "); Serial.println(freq);
  Serial.print("period: "); Serial.println(period);
  Serial.print("byte: "); Serial.println(sizeof(byte));
  Serial.print("int: "); Serial.println(sizeof(int));
  Serial.print("long: "); Serial.println(sizeof(long));
}


// interrupt routine
static void PWM_Accumulate() {
  if (pwmInfo.dutyCount < MAX_PWM_DUTY){
    pwmInfo.dutyCount ++;
  }
  else{
    pwmInfo.dutyCount = 0;

    // all pins starts at HIGH output
//    for (int i=0; i<pwmInfo.n; i++) {
//     if (0<pwmInfo.dutyCycle[i]) 
//        digitalWrite(pwmInfo.pin[i], HIGH);
//    }
  }
}


void PWM_Toggle() {
  if ((0 == pwmInfo.dutyCount) ){
    for (int i=0; i<pwmInfo.n; i++) {
      if (0 < pwmInfo.dutyCycle[i]) {digitalWrite(pwmInfo.pin[i], HIGH);}
    }
    return;
  }
  
  // toggle to LOW when count hits the dutyCycle
  for (int i=0; i < pwmInfo.n; i++) {
    if (pwmInfo.dutyCount > pwmInfo.dutyCycle[i]){
      digitalWrite(pwmInfo.pin[i], LOW);
    }
  }
}


void PWM_Set(byte pin, byte duty){
  // check repetition
  for (int i=0; i<pwmInfo.n; i++){
    if (pin == pwmInfo.pin[i]){
      pwmInfo.dutyCycle[i] = duty;
      return;
    }
  }

  // when no repeat
  if (pwmInfo.n < MAX_PWM_PIN) {
    pwmInfo.pin[pwmInfo.n] = pin;
    pwmInfo.dutyCycle[pwmInfo.n] = duty;
    pwmInfo.n ++;
  }
}


void PWM_Check(){
  Serial.println("PWM Check");
  Serial.print("n: ");  Serial.println(pwmInfo.n);
  Serial.print("dutyCount: ");  Serial.println(pwmInfo.dutyCount);
  Serial.println("pin, dutyCycle: ");
  for (int i=0; i<pwmInfo.n; i++){
    Serial.print(pwmInfo.pin[i]);
    Serial.print(" | ");
    Serial.println(pwmInfo.dutyCycle[i]);
  }
}

// ================================================
// BLDC 
// ================================================
#define INH_A  2
#define IN_A   3
#define INH_B  4
#define IN_B   5
#define INH_C  6
#define IN_C   7
#define HALL_A 8
#define HALL_B 9
#define HALL_C 10


const int INH[] = {INH_A, INH_B, INH_C};
const int IN[]  = {IN_A, IN_B, IN_C};
const int HALL[] = {HALL_A, HALL_B, HALL_C};


void InitBLDC()
{
  for (int i=0; i<3; i++){
    pinMode(INH[i], OUTPUT);
    digitalWrite(INH[i], LOW);
    pinMode(IN[i], OUTPUT);
    digitalWrite(IN[i], LOW);
    pinMode(HALL[i], INPUT_PULLUP);
  }
}


void BLDCSet(const byte inh[], const byte in[], int dutyCycle){
  for (int i=0; i<3; i++){
    (1 == inh[i]) ? PWM_Set(INH[i], dutyCycle) : PWM_Set(INH[i], 0);
    digitalWrite(IN[i], in[i]);
  }
}


void BLDCSet_NoPWM(const byte inh[], const byte in[]){
  for (int i=0; i<3; i++){
    digitalWrite(INH[i], inh[i]);
    digitalWrite(IN[i], in[i]);
  }
}


// ================================================
// Try to switch the BLDC when any hall changes
// ================================================

int g_idx_step = 0;
// pin status on each step
byte steps[6][6] = {  // phase in INH_A, IN_A, INH_B, IN_B, INH_C, IN_C
  {1,1,1,0,0,0},  // AB
  {1,1,0,0,1,0},  // AC
  {0,0,1,1,1,0},  // BC
  {1,0,1,1,0,0},  // BA
  {1,0,0,0,1,1},  // CA
  {0,0,1,0,1,1}  // CB
};


void NextStep(byte inh[], byte in[]){
  g_idx_step++;
  if (g_idx_step >= 6){
    g_idx_step = 0;
  }

  // copy state of current
  for (int i=0; i<3; i++){
    inh[i] = steps[g_idx_step][2*i];
    in[i] = steps[g_idx_step][2*i+1];
  }
}


bool IsHallChanged(){
  static byte last_hall[3] = {0};
  for (int i=0; i<3; i++) {
    byte this_hall = digitalRead(HALL[i]);
    if (this_hall != last_hall[i]){
      // only one hall changes at once, so update the changed one is enough
      last_hall[i] = this_hall;
      return true;
    }
  }
  return false;
}


void PrintStep(const byte inh[], const byte in[]){
  Serial.print("step & hall: ");
  for (int i=0; i<3; i++){
    Serial.print(inh[i]); Serial.print(in[i]);
    Serial.print(" ");
  }
  
  for (int i=0; i<3; i++){
    Serial.print(digitalRead(HALL[i]));
  }
  Serial.println();
}


void setup() {
  Serial.begin(115200);
  InitBLDC();
  PWM_Init(100);
}


byte inh[3], in[3];
long lastTime = millis();
long thisTime = lastTime;
const unsigned int interval = 5000;
byte g_dutyCycle = 0;

void loop() {
  thisTime = millis();
  if ((thisTime - lastTime) > interval){
    lastTime = thisTime;
    NextStep(inh, in);

    // increase dutyCycle
    g_dutyCycle += 0.3 * MAX_PWM_DUTY;
    if (g_dutyCycle > MAX_PWM_DUTY) {
      g_dutyCycle = 0.05 * MAX_PWM_DUTY;
    }
  }


  // routine 
  if (IsHallChanged()){
    NextStep(inh, in);
    BLDCSet(inh, in, g_dutyCycle);
  }

  PWM_Toggle();  
}
